#ifndef YAW_ANGLE_CONTROLLER__YAW_CONTROLLER_HPP_
#define YAW_ANGLE_CONTROLLER__YAW_CONTROLLER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <std_msgs/msg/float64.hpp>
#include <tf2_ros/transform_listener.hpp>
#include <tf2_ros/buffer.hpp>
#include <tf2/utils.h>

namespace yaw_angle_controller
{

class PIDController
{
public:
  PIDController(double kp, double ki, double kd, double max_output, double min_output);
  
  double compute(double setpoint, double current_value, double dt);
  void reset();
  void setGains(double kp, double ki, double kd);

private:
  double kp_, ki_, kd_;
  double max_output_, min_output_;
  double integral_;
  double previous_error_;
  bool first_call_;
};

class YawController : public rclcpp::Node
{
public:
  YawController();

private:
  void timerCallback();
  void targetYawCallback(const std_msgs::msg::Float64::SharedPtr msg);
  double getCurrentYaw();
  double normalizeAngle(double angle);
  void publishVelocity(double angular_velocity);

  // ROS2 components
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr target_yaw_subscriber_;
  
  // TF2
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  
  // PID Controller
  std::unique_ptr<PIDController> pid_controller_;
  
  // Parameters
  std::string base_frame_;
  std::string map_frame_;
  double control_frequency_;
  double target_yaw_;
  double yaw_tolerance_;
  double max_angular_velocity_;
  double min_angular_velocity_;
  
  // PID gains
  double kp_, ki_, kd_;
  
  rclcpp::Time last_time_;
};

}  // namespace yaw_angle_controller

#endif  // YAW_ANGLE_CONTROLLER__YAW_CONTROLLER_HPP_